#!/usr/bin/env python3
from __future__ import absolute_import
import rospy
from nav_msgs.msg import Path
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseStamped

def leader_odom_cb(data):
    global leader_odom
    leader_odom = data

def follower1_odom_cb(data):
    global follower1_odom
    follower1_odom = data

def follower2_odom_cb(data):
    global follower2_odom
    follower2_odom = data

def node():
    global leader_odom
    global follower1_odom
    global follower2_odom

    rospy.Subscriber('/tb3_0/odom', Odometry, leader_odom_cb, queue_size=1, buff_size=52428800)
    rospy.Subscriber('/tb3_1/odom', Odometry, follower1_odom_cb, queue_size=1, buff_size=52428800)
    rospy.Subscriber('/tb3_2/odom', Odometry, follower2_odom_cb, queue_size=1, buff_size=52428800)

    leader_odom_pub = rospy.Publisher('/tb3_0/fake_odom', Odometry, queue_size=1)
    follower1_odom_pub = rospy.Publisher('/tb3_1/fake_odom', Odometry, queue_size=1)
    follower2_odom_pub = rospy.Publisher('/tb3_2/fake_odom', Odometry, queue_size=1)
    rate = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():
        pub_leader_odom = leader_odom
        pub_follower1_odom = follower1_odom
        pub_follower2_odom = follower2_odom

        pub_leader_odom.pose.pose.position.x = pub_leader_odom.pose.pose.position.x
        pub_leader_odom.pose.pose.position.y = pub_leader_odom.pose.pose.position.y

        pub_follower1_odom.pose.pose.position.x = pub_follower1_odom.pose.pose.position.x - 1.0
        pub_follower1_odom.pose.pose.position.y = pub_follower1_odom.pose.pose.position.y - 1.0

        pub_follower2_odom.pose.pose.position.x = pub_follower2_odom.pose.pose.position.x - 1.0
        pub_follower2_odom.pose.pose.position.y = pub_follower2_odom.pose.pose.position.y + 1.0

        leader_odom_pub.publish(pub_leader_odom)
        follower1_odom_pub.publish(pub_follower1_odom)
        follower2_odom_pub.publish(pub_follower2_odom)

        rate.sleep()

if __name__ == '__main__':
    rospy.init_node('fake_odom_node')
    rospy.loginfo("fake odom node!!")

    leader_odom = Odometry()
    follower1_odom = Odometry()
    follower2_odom = Odometry()

    try:
        node()
    except rospy.ROSInterruptException:
        pass  
